#include "feature.h"
#include "utils.h"
#include <limits>

using namespace cv;
using namespace std;

struct cmp {
	bool operator()(vector<cv::Point> contour1, vector<cv::Point> contour2 ) {
		double i = fabs( contourArea(cv::Mat(contour1)) );
		double j = fabs( contourArea(cv::Mat(contour2)) );
		return ( i > j );
	}
};


void calcRadAngleHist(const cv::Mat & image, cv::Mat& hist, int nbins)
{
	std::vector<cv::Point> cont;
	findMaxContour(image, cont);

	if (cont.empty())
	{	
		hist = cv::Mat();
		return;
	}

	//cv::Mat contours_img = cv::Mat::zeros(image.size(), CV_8U);
	//cv::drawContours(contours_img, contour, maxInd, 255);

	cv::Moments moments;
	moments = cv::moments(cont);

	cv::Point2f center;
	center.x = moments.m10 / moments.m00;
	center.y = moments.m01 / moments.m00;
	cv::Point2f anchor = cont[0];

	//cv::circle(contours_img, center, 10, 150, 5);
	//cv::circle(contours_img, anchor, 10, 150, -1);

	hist = cv::Mat::zeros(1, nbins, CV_32F);
	float * hist_row = hist.ptr<float>(0);
	
	for(int i = 0; i < cont.size(); i++)
	{
		float angle = calcAngle(anchor, center, cont[i]);
		int ind = angle / 360. * nbins;
		hist_row[ind] += cv::norm(cv::Vec2f(center.x, center.y), cv::Vec2f(cont[i].x, cont[i].y));
	}

	hist /= sum(hist)[0];

	cv::Scalar mean, stddev;
	cv::meanStdDev(hist, mean, stddev);

	float border_val = (hist.at<float>(0) + hist.at<float>(1) + hist.at<float>(70) + hist.at<float>(71)) * 0.25;
	if(mean[0] - border_val > 0)
	{
		cv::Mat hist_ = hist.clone();
		for(int i = 0; i < nbins; i++)
			hist_.at<float>((i + nbins / 2) % nbins) = hist.at<float>(i);
		hist = hist_.clone();
		std::cout << "swap" << std::endl;
	}

//#ifdef _DEBUG
//	cv::imshow("contours", contours_img);
//#endif // _DEBUG
}

void showHist(const cv::Mat& hist)
{
	double maxVal;
	cv::minMaxIdx(hist, 0, &maxVal);

	cv::Mat display = cv::Mat::zeros(310, 360, CV_8U);

	double ratio_y = 310. / maxVal;
	double ratio_x = 360. / hist.cols;

	const float * hist_row = hist.ptr<float>(0);
	for (int i = 1; i < hist.cols; i++)
	{
		cv::Point p1(i * ratio_x, display.rows - hist_row[i] * ratio_y);
		cv::Point p2((i - 1) * ratio_x, display.rows - hist_row[i - 1] * ratio_y);
		cv::line(display, p1, p2, 255);
	}

	cv:imshow("hist", display);
}


void align(cv::Mat & img)
{
	cv::Mat image = img.clone();
	cv::resize(img, image, cv::Size(), 0.25, 0.25);
	std::vector<cv::Point> cont;
	findMaxContour(image, cont);

	cv::Moments moments;
	moments = cv::moments(cont);

	cv::Point2f center;
	center.x = moments.m10 / moments.m00;
	center.y = moments.m01 / moments.m00;
	cv::Point2f anchor = cont[0];

	cv::Mat blurred = image.clone();
	//cv::blur(image, blurred, cv::Size(21, 21));

  	double minDiff = FLT_MAX;
	double minAngle = 0;
	for(int phi = 0; phi < 180; phi+=2)
	{
		cv::Mat flipped = flipLine(blurred, center, phi);
		double diff = cv::norm(blurred, flipped);
		//std::cout << diff << std::endl;
		//cv::imshow("flipped", abs(flipped - blurred));
		//cv::waitKey(0);
		if(diff < minDiff)
		{
			minDiff = diff;
			minAngle = phi;
		}      
	}

    //std::cout << minAngle << std::endl;

	cv::Mat rot_mat = getRotationMatrix2D(center * 4, std::min(90. - minAngle, 90 + minAngle), 1.0);
	warpAffine(img, img, rot_mat, img.size());
}

void calcShapeFeatures(const cv::Mat & image, cv::Mat& features, int nbins)
{
	features = cv::Mat_<float>(1, nbins);
	std::vector<cv::Point> cont;
	findMaxContour(image, cont);

	cv::Moments moment;
	moment = cv::moments(image, true);

	float eccentricity = sqrt((moment.mu20 - moment.mu02) * (moment.mu20 - moment.mu02) + 4. * moment.mu11 * moment.mu11) /
		               (moment.mu20 + moment.mu02);

	cv::RotatedRect rect = cv::minAreaRect(cont);

	float aspect_ratio = rect.size.height / rect.size.width;

	float elongation = 1 - 2. * radInscribedCircle(image) /  rect.size.height; 

	std::vector<cv::Point> hullPts;
	std::vector<int> hull;
	cv::convexHull(cont, hullPts, false, true);
	cv::convexHull(cont, hull, false, false);

	float solidity = cv::contourArea(cont) / cv::contourArea(hullPts);

	float isoperimetric = 4 * CV_PI * cv::contourArea(cont) / (cv::arcLength(cont, true) * cv::arcLength(cont, true));

	features.ptr<float>(0)[0] = eccentricity;
	features.ptr<float>(0)[1] = aspect_ratio;
	features.ptr<float>(0)[2] = elongation;
	features.ptr<float>(0)[3] = solidity;
	features.ptr<float>(0)[4] = isoperimetric;

	//std::vector<cv::Vec4i> defects;
	//cv::convexityDefects(cont, hull, defects);

	//std::vector<float> lenDefects;
	//for(int i = 0; i < defects.size(); i++)
	//	lenDefects.push_back(defects[i][3] / 256.);

	//std::sort(lenDefects.begin(), lenDefects.end());

	//cv::Scalar mean, stddev;
	//cv::meanStdDev(lenDefects, mean, stddev);

	//for(int i = 0; i < defects.size(); i++)
	//	lenDefects[i] -=mean [0];

	//cv::Mat fourierCoeff;
	//cv::dft(lenDefects, fourierCoeff, cv::DFT_SCALE);

}


/*
parse_file:
Load file, read it and fill featuresMatrix using extractFeatures func
*/
void parseFileAndExtractFeatures(const char* filename, cv::Mat& featuresMatrix, cv::Mat& responses, int nFeatures, extractFeaturesFunc extractFeatures)
{
	std::string line;
	std::ifstream myfile;
	int total_number, i=0, columns_number=3;
	myfile.open(filename, std::ios::in);

	getline(myfile, line);
	total_number = atoi(line.c_str());
	getline(myfile, line);  //Skip header

	featuresMatrix = cv::Mat(total_number, nFeatures, CV_32F);
	responses = cv::Mat();

	while (getline(myfile, line))
	{
		std::stringstream strstr(line);
		std::string word = "";
		while (getline(strstr, word, ';'))
		{
			if (i % columns_number == 0) //Filename
			{
				cout << i/columns_number << endl;
				cv::Mat rowForFeature = featuresMatrix(cv::Range(i/columns_number, i/columns_number + 1), cv::Range::all()); 
				cv::Mat image = cv::imread(word.c_str());
				if(image.empty())
					continue;

				cv::Mat gray, bin, hist;
				//if(image.depth() == 3)
				cvtColor(image, gray, cv::COLOR_BGR2GRAY);
				//else
				//	gray = image;
				//threshold(gray, bin, 150, 255, THRESH_BINARY_INV);
				bin = gray;
				resize(bin, bin, Size(), 0.5, 0.5);
				//align(bin);
				extractFeatures(bin, rowForFeature, nFeatures);
				if(rowForFeature.empty())
					continue;

				//showHist(rowForFeature);
				//cv::imshow("bin", bin);
				//cv::waitKey(3);
			}
			else if (i % columns_number == 1) //Class name
			{

			}
			else if (i % columns_number == 2) //Class id
			{
				int classId = atoi(word.c_str());
				responses.push_back((float)classId);
			}
			i++;
		}
	}
	cout << "finished reading" << endl;
	myfile.close();
}

int qqcount = 0;

void calcHoCS(const cv::Mat & image, cv::Mat& features, int nb)
{
	vector<vector<Point> > contours;
	findContours(image.clone(), contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
	sort(contours.begin(), contours.end(), cmp());
	vector<Point> & cont = contours[0];

	//cv::approxPolyDP(cont, cont, arcLength(cont, true) * 1e-4, true);

	Mat bin = Mat::zeros(image.size(), CV_8U);

	cv::drawContours(bin, contours, 0, 255, - 1);
	resize(bin, bin, Size(), 0.25, 0.25, CV_INTER_AREA);

	//cv::imshow("before", bin);

	Mat bin_tmp;
	removeStem(bin, bin_tmp);
	crop(bin_tmp, bin);

	//cv::imshow("after", bin);
	//cv::waitKey(0);

	int HOCS_A = 2, HOCS_B = 3, HOCS_NB = 21, HOCS_NSC = 25;

	int nbins = HOCS_NB;
	int nscales = HOCS_NSC;
	int a = HOCS_A;
	int b = HOCS_B;
	int ncontpts = cont.size();

	Mat areaImage = Mat::zeros(nscales, ncontpts, CV_32F);
	Mat angleImage = Mat::zeros(nscales, ncontpts, CV_32F);

	for(int s = 0; s < nscales; s++)
	{
		int radius = s * a + b;
		Rect border(radius, radius, bin.cols - 2 * radius, bin.rows - 2 * radius);
		float * areaImage_row = areaImage.ptr<float>(s);
		float * angleImage_row = angleImage.ptr<float>(s);
		for(int p = 0; p < ncontpts; p++)
		{
			Mat mask = Mat::zeros(radius * 2 + 1, radius * 2 + 1, CV_8U);
			Point & cur = cont[p];
			if(!border.contains(cur))
				continue;

			circle(mask, Point(radius, radius), radius, 255, -1);
			float circleArea = countNonZero(mask);
			mask &= bin(Rect(cur.x - radius, cur.y - radius, 2 * radius + 1, 2 * radius + 1));

			float segmentArea = countNonZero(mask);
			areaImage_row[p] = segmentArea / circleArea;
			mask.setTo(0);

			circle(mask, Point(radius, radius), radius, 255, 1, 8);
			float circleLen = countNonZero(mask);
			mask &= bin(Rect(cur.x - radius, cur.y - radius, 2 * radius + 1, 2 * radius + 1));

			float arcLen = countNonZero(mask);
			angleImage_row[p] = arcLen / circleLen;

			/*Point & cur = cont[p];
			mask.setTo(0);
			circle(mask, cur, radius, 255, -1);
			float circleArea = countNonZero(mask);
			mask &= bin;
			float segmentArea = countNonZero(mask);
			areaImage_row[p] = segmentArea / circleArea;

			mask.setTo(0);
			circle(mask, cur, radius, 255, 1, 8);
			float circleLen = countNonZero(mask);
			mask &= bin;
			float arcLen = countNonZero(mask);
			angleImage_row[p] = arcLen / circleLen;*/

		}
	}

	Mat areaHist = Mat::zeros(nscales, nbins, CV_32F);
	Mat angleHist = Mat::zeros(nscales, nbins, CV_32F);

	double maxVal, minVal;
	minMaxIdx(areaImage, &minVal, &maxVal);
	//cout << maxVal << " " << minVal << endl;

	//TODO linear interpolation (bilinear?)

	for(int s = 0; s < nscales; s++)
	{
		float * areaImage_row = areaImage.ptr<float>(s);
		float * areaHist_row = areaHist.ptr<float>(s);
		for(int p = 0; p < ncontpts; p++)
		{
			//float y = areaImage_row[p] * nbins;
			//float a = floor(2 * y);
			//int idx1;
			//if(a == 0) idx1 = 0;
			//else if(a == 2 * nbins - 1) idx1 = nbins - 2;
			//else idx1 = (a - 1) / 2;
			//int idx2 = idx1 + 1;

			//float x1 = idx1 / nbins + 1 / (nbins * 2);
			//areaHist_row[idx1] += 1 - (areaImage_row[p] - x1) * nbins;
			//areaHist_row[idx2] += (areaImage_row[p] - x1) * nbins;

			//int idx1 = floor(areaImage_row[p] * nbins);
			//int idx2 = ceil(areaImage_row[p] * nbins);
			//areaHist_row[idx1] += 1 - (areaImage_row[p] - (idx1 / nbins) + 2 / nbins) * nbins;
			int idx = cvRound(areaImage_row[p] * nbins);
			areaHist_row[idx]++;
		}
	}

	minMaxIdx(angleImage, &minVal, &maxVal);
	//cout << maxVal << " " << minVal << endl;

	for(int s = 0; s < nscales; s++)
	{
		float * angleImage_row = angleImage.ptr<float>(s);
		float * angleHist_row = angleHist.ptr<float>(s);
		for(int p = 0; p < ncontpts; p++)
		{
			int idx = cvRound(angleImage_row[p] * nbins);
			angleHist_row[idx] ++;
		}
	}

	

	areaHist /= sum(areaHist)[0];
	angleHist /= sum(angleHist)[0];
	//normalize(areaHist, areaHist, 1, 0, NORM_L2);
	//normalize(angleHist, angleHist, 1, 0, NORM_L2);

	//cout << areaHist << endl;
	//cout << angleHist << endl;

	//Mat showArea(areaHist.size(), CV_8U);
	//normalize(areaHist, showArea, 0, 255, CV_MINMAX);

	//Mat showAngle(angleHist.size(), CV_8U);
	//normalize(angleHist, showAngle, 0, 255, CV_MINMAX);

	//resize(showAngle, showAngle, Size(), 8, 8, CV_INTER_NN);
	//resize(showArea, showArea, Size(), 8, 8, CV_INTER_NN);

	//imshow("area", showArea);
	//imshow("angle", showAngle);

	//imwrite(format("areaHist%i.png",qqcount), showArea);
	//imwrite(format("angleHist%i.png", qqcount++), showAngle);
	//cv::waitKey(0);

	areaHist.push_back(angleHist);
	Mat hist(1, areaHist.total(), CV_32F, areaHist.ptr(0));
	hist.copyTo(features);
}

void removeStem(const Mat & bin, Mat & dst)
{
	Mat mask;
	cv::erode(bin, mask, Mat(), Point(-1, -1), 2);
	cv::dilate(mask, mask, Mat(), Point(-1, -1), 2);

	std::vector<cv::Point> contour;
	findMaxContour(mask, contour);
	mask.setTo(0);

	vector<vector<Point> > c(1);
	c[0] = contour;
	cv::drawContours(mask, c, 0, 255, -1);

	//cv::imshow("mask", mask);

	dst = bin & mask;

}